#include <string>
#include <iostream>
#include <fstream>
#include "openGA.hpp"

using std::string;
using std::cout;
using std::endl;

struct CR_Solution
{
	double x;
	double y;
	double z;
	double pit;
	double yaw;
	double rol;

	string to_string() const
	{
		return 
			string("{")
			+  "x:"+std::to_string(x)
			+", y:"+std::to_string(y)
			+", z:"+std::to_string(z)
			+", pit:"+std::to_string(pit)
			+", yaw:"+std::to_string(yaw)
			+", rol:"+std::to_string(rol)
			+"}";
	}
};

struct CR_MiddleCost
{
	// This is where the results of simulation
	// is stored but not yet finalized.
	double objective6;
};

typedef EA::Genetic<CR_Solution,CR_MiddleCost> GA_Type;
typedef EA::GenerationType<CR_Solution,CR_MiddleCost> Generation_Type;

void init_genes(CR_Solution& p,const std::function<double(void)> &rnd01)
{
	// rnd01() gives a random number in 0~1
	p.x=0.0+10*rnd01();
	p.y=0.0+10*rnd01();
	p.z=0.0+10*rnd01();
	p.pit=0.0+10*rnd01();
	p.yaw=0.0+10*rnd01();
	p.rol=0.0+10*rnd01();
}

bool eval_solution(
	const CR_Solution& p,
	CR_MiddleCost &c)
{
	const double& x=p.x;
	const double& y=p.y;
	const double& z=p.z;
	const double& pit=p.pit;
	const double& yaw=p.yaw;
	const double& rol=p.rol;

	c.objective6=cos(y)*exp(x)+sin(pit)*log(rol);
	return true; // solution is accepted
}

CR_Solution mutate(
	const CR_Solution& X_base,
	const std::function<double(void)> &rnd01,
	double shrink_scale)
{
	CR_Solution X_new;
	const double mu = 0.2*shrink_scale; // mutation radius (adjustable)
	bool in_range;
	do{
		in_range=true;
		X_new=X_base;
		X_new.x+=mu*(rnd01()-rnd01());
		in_range=in_range&&(X_new.x>=0.0 && X_new.x<10.0);
		X_new.y+=mu*(rnd01()-rnd01());
		in_range=in_range&&(X_new.y>=0.0 && X_new.y<10.0);
		X_new.z+=mu*(rnd01()-rnd01());
		in_range=in_range&&(X_new.z>=0.0 && X_new.z<10.0);
		X_new.pit+=mu*(rnd01()-rnd01());
		in_range=in_range&&(X_new.pit>=0.0 && X_new.pit<10.0);
		X_new.yaw+=mu*(rnd01()-rnd01());
		in_range=in_range&&(X_new.yaw>=0.0 && X_new.yaw<10.0);
		X_new.rol+=mu*(rnd01()-rnd01());
		in_range=in_range&&(X_new.rol>=0.0 && X_new.rol<10.0);
	} while(!in_range);
	return X_new;
}

CR_Solution crossover(
	const CR_Solution& X1,
	const CR_Solution& X2,
	const std::function<double(void)> &rnd01)
{
	CR_Solution X_new;
	double r;
	r=rnd01();
	X_new.x=r*X1.x+(1.0-r)*X2.x;
	r=rnd01();
	X_new.y=r*X1.y+(1.0-r)*X2.y;
	r=rnd01();
	X_new.z=r*X1.z+(1.0-r)*X2.z;
	r=rnd01();
	X_new.pit=r*X1.pit+(1.0-r)*X2.pit;
	r=rnd01();
	X_new.yaw=r*X1.yaw+(1.0-r)*X2.yaw;
	r=rnd01();
	X_new.rol=r*X1.rol+(1.0-r)*X2.rol;
	return X_new;
}

double calculate_SO_total_fitness(const GA_Type::thisChromosomeType &X)
{
	// finalize the cost
	double final_cost=0.0;
	final_cost+=X.middle_costs.objective6;
	return final_cost;
}

std::ofstream output_file;

void SO_report_generation(
	int generation_number,
	const EA::GenerationType<CR_Solution,CR_MiddleCost> &last_generation,
	const CR_Solution& best_genes)
{
	cout
		<<"Generation ["<<generation_number<<"], "
		<<"Best="<<last_generation.best_total_cost<<", "
		<<"Average="<<last_generation.average_cost<<", "
		<<"Best genes=("<<best_genes.to_string()<<")"<<", "
		<<"Exe_time="<<last_generation.exe_time
		<<endl;

	output_file
		<<generation_number<<"\t"
		<<last_generation.average_cost<<"\t"
		<<last_generation.best_total_cost<<"\t"
		<<best_genes.to_string()<<"\n";
}

int main()
{
	output_file.open("results.txt");
	output_file<<"step"<<"\t"<<"cost_avg"<<"\t"<<"cost_best"<<"\t"<<"solution_best"<<"\n";

	EA::Chronometer timer;
	timer.tic();

	GA_Type ga_obj;
	ga_obj.problem_mode=EA::GA_MODE::SOGA;
	ga_obj.multi_threading=true;
	ga_obj.idle_delay_us=10; // switch between threads quickly
	ga_obj.dynamic_threading=true;
	ga_obj.verbose=false;
	ga_obj.population=200;
	ga_obj.generation_max=1000;
	ga_obj.calculate_SO_total_fitness=calculate_SO_total_fitness;
	ga_obj.init_genes=init_genes;
	ga_obj.eval_solution=eval_solution;
	ga_obj.mutate=mutate;
	ga_obj.crossover=crossover;
	ga_obj.SO_report_generation=SO_report_generation;
	ga_obj.crossover_fraction=0.7;
	ga_obj.mutation_rate=0.2;
	ga_obj.best_stall_max=10;
	ga_obj.elite_count=10;
	ga_obj.solve();

	cout<<"The problem is optimized in "<<timer.toc()<<" seconds."<<endl;

	output_file.close();
	return 0;
}